#ifndef __NODE__
#define __NODE__


#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include <iostream>
#include <ctime>
#include <stdlib.h>
#include <cstdlib>
#include <math.h>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <utility>
#include <stack>
#include <queue>
#include <set>

using namespace std;

struct Node{
  public:
  bduVec3f* foot_pos; // pointer on bduVec3f : will be size 4
  int moving_foot; //number of the current moving foot
  float* center; //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
  float h; // heuristic
  float g; // cost
  float f; // f=g+h is used to compare nodes
  Node* parent;
  SDSimulatorState* state;
  int moved_foot;
  float moved_foot_x;
  float moved_foot_y;
  float zoff; //its the zoffset used to tell if the dog flipped - 
                //if its higher this node has a chance of being a "phantom flipper"
  const static int features = 26;
  float costs[features];

  Node(){
     moving_foot = 0;
     h = 0;
     g = 0;
     f = 0;
  }
  /*~Node(){
     delete center;
     delete foot_pos;
  }*/
};

    float* FindNodeCenter(Node* node);
    float FootDist(Node* node, int foot1, int foot2);
    void PrintFootPositions(Node* node);

struct step{ //the instructions necessary for a single robot step
  public:
     int foot;
     float x;
     float y;

     step(Node* targetNode){
        foot = targetNode->moving_foot;
        x = targetNode->foot_pos[foot].n[0];
        y = targetNode->foot_pos[foot].n[1];
     }
     
};


class smallest_f_node // comparison function for priority queue
{
public:
  bool operator() (Node* lhs, Node* rhs) const
  {
    return (lhs->f > rhs->f);
  }
};

class smallest_node_node // comparison function for multiset
{
public:
  bool operator() (Node* lhs, Node* rhs) const
  {
     float d = 0.01;
     float gridSize = d/2; //Grid size used for determining same states.
     int Xl = floor(lhs->center[0]/gridSize);
     int Yl = floor(lhs->center[1]/gridSize);
     int Xr = floor(rhs->center[0]/gridSize);
     int Yr = floor(rhs->center[1]/gridSize);
     bool answer = (Xl < Xr) || ((Xr == Xl) && (Yl < Yr));
     return answer;
  }
};


#endif
